﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

using System.Runtime.InteropServices;
using System.Windows.Forms;
using System.IO.Ports;
using System.Threading;

namespace Ultrasonic
{
    static class BoardCMD
    {
        /// <summary>
        /// 比较两个byte数组是否相等
        /// </summary>
        /// <param name="b1"></param>
        /// <param name="b2"></param>
        /// <returns></returns>
        public static int BytesCompare(byte[] b1, byte[] b2,int length)
        {
            int result = 0;
            for (int i = 0; i < length; i++)
            {
                if (b1[i] != b2[i])
                {
                    result = (int)(b1[i] - b2[i]);
                    break;
                }
            }
            return result;
        }
        /// <summary>
        /// 将不带参数的命令计算好CRC并转为byte数组。
        /// </summary>
        /// <param name="board_cmd"></param>
        /// <returns></returns>
        public static byte[] CmdNoParamWithCrcBytes(byte board_cmd)
        {
            UltrasonicGlobal.UsbPrefix prefix = new UltrasonicGlobal.UsbPrefix();
            prefix.addr = UltrasonicGlobal.ARTIFICIAL_BOARD_ADDR;
            prefix.funcCode = board_cmd;
            prefix.length = (UInt16)(Marshal.SizeOf(prefix) + 2);
            byte[] prefixBytes = UltrasonicGlobal.StructToBytes(prefix, Marshal.SizeOf(prefix));
            ushort calCrc = (ushort)CRC16.calcCrc16(prefixBytes, 0, prefixBytes.Length);
            byte[] crcBytes = BitConverter.GetBytes(calCrc);
            byte[] byteToWrite = UltrasonicGlobal.MergeByte(prefixBytes, crcBytes);
            return byteToWrite;
        }

        public static byte[] CmdWithParamAndCrcBytes(byte board_cmd,byte[] structBytes)
        {
            UltrasonicGlobal.UsbPrefix prefix = new UltrasonicGlobal.UsbPrefix();
            prefix.addr = UltrasonicGlobal.ARTIFICIAL_BOARD_ADDR;
            prefix.funcCode = board_cmd;
            prefix.length = (UInt16)(Marshal.SizeOf(prefix)+structBytes.Length + 2);

            byte[] prefixBytes = UltrasonicGlobal.StructToBytes(prefix, Marshal.SizeOf(prefix));
            byte[] sumBytes = UltrasonicGlobal.MergeByte(prefixBytes, structBytes);
            ushort calCrc = (ushort)CRC16.calcCrc16(sumBytes, 0, sumBytes.Length);
            byte[] crcBytes = BitConverter.GetBytes(calCrc);
            byte[] byteToWrite = UltrasonicGlobal.MergeByte(sumBytes, crcBytes);
            return byteToWrite;
        }
        /// <summary>
        /// 握手信号的byte数组
        /// </summary>
        /// <returns></returns>
        public static byte[] CMD_BOARD_HANDSHAKE_SEND()
        {
            return CmdNoParamWithCrcBytes(UltrasonicGlobal.CMD_BOARD_HANDSHAKE);
        }
        /// <summary>
        /// 打开角磨机信号byte数组
        /// </summary>
        /// <returns></returns>
        public static byte[] CMD_MILL_ON_SEND()
        {
            return CmdNoParamWithCrcBytes(UltrasonicGlobal.CMD_MILL_ON);
        }
        /// <summary>
        /// 关闭角磨机信号byte数组
        /// </summary>
        /// <returns></returns>
        public static byte[] CMD_MILL_OFF_SEND()
        {
            return CmdNoParamWithCrcBytes(UltrasonicGlobal.CMD_MILL_OFF);
        }
        /// <summary>
        /// 设置左右轮为同一个速度
        /// </summary>
        /// <returns></returns>
        public static byte[] CMD_SET_WHEEL_SAME_SPEED_SEND()
        {
            return CmdNoParamWithCrcBytes(UltrasonicGlobal.CMD_SET_WHEEL_SAME_SPEED);
        }
        /// <summary>
        /// 设置电机的速度，此函数在连续旋转模式下使用
        /// </summary>
        /// <param name="setSpeedStruct"></param>
        /// <returns></returns>
        public static byte[] CMD_SET_WHEEL_SPEED_SEND(UltrasonicGlobal.SetWheelSpeed setSpeedStruct)
        {
            byte[] stuctBytes = UltrasonicGlobal.StructToBytes(setSpeedStruct, Marshal.SizeOf(setSpeedStruct));
            return CmdWithParamAndCrcBytes(UltrasonicGlobal.CMD_SET_WHEEL_SPEED, stuctBytes);
        }
        /// <summary>
        /// 设置电机单个内存地址的参数
        /// </summary>
        /// <param name="setSpeedStruct"></param>
        /// <returns></returns>
        public static byte[] CMD_SET_MOTOR_MEM_PARAMETER_SEND(UltrasonicGlobal.SetMotorMemParameter setMemParamStruct)
        {
            byte[] stuctBytes = UltrasonicGlobal.StructToBytes(setMemParamStruct, Marshal.SizeOf(setMemParamStruct));
            return CmdWithParamAndCrcBytes(UltrasonicGlobal.CMD_SET_MOTOR_MEM_PARAMETER, stuctBytes);
        }
        /// <summary>
        /// 设置电机位置
        /// </summary>
        /// <param name="setMotorPosStruct"></param>
        /// <returns></returns>
        public static byte[] CMD_SET_MOTOR_POSITION_SEND(UltrasonicGlobal.SetMotorPosition setMotorPosStruct)
        {
            byte[] stuctBytes = UltrasonicGlobal.StructToBytes(setMotorPosStruct, Marshal.SizeOf(setMotorPosStruct));
            return CmdWithParamAndCrcBytes(UltrasonicGlobal.CMD_SET_MOTOR_POSITION, stuctBytes);
        }

        public static byte[] CMD_SET_MOTOR_ACCELERATE_SEND(UltrasonicGlobal._SetMotorAccelerate setMotorAccelerateStruct)
        {
            byte[] stuctBytes = UltrasonicGlobal.StructToBytes(setMotorAccelerateStruct, Marshal.SizeOf(setMotorAccelerateStruct));
            return CmdWithParamAndCrcBytes(UltrasonicGlobal.CMD_SET_MOTOR_ACCELERATE, stuctBytes);
        }
    }
}
